#include <ipa_building_navigation/nearest_neighbor_TSP.h>

//Default Constructor
NearestNeighborTSPSolver::NearestNeighborTSPSolver()
{

}

// This function calculates the order of the TSP, using the nearest neighbor method.
// It uses a pathlength Matrix, which should be calculated once.
// This Matrix should save the pathlengths with this logic:
//		1. The rows show from which Node the length is calculated.
//		2. For the columns in a row the Matrix shows the distance to the Node in the column.
//		3. From the node to itself the distance is 0.
// 最短距离搜索策略则是：
//（1）先将两点之间连一条直线，计算该直线上是否穿过障碍物，假如没有穿过障碍物，则直线距离即为两点之间的最短距离；
//（2）假如两点之间的连线穿过障碍物，则使用A*算法计算出两点之间的最短距离。
// 计算生成距离矩阵后，就可以通过距离矩阵，计算得出最邻近点的遍历序列。
// 需要注意的是，这段代码实现存在一个隐患，上面已经讲到，调用该接口默认会将地图进行缩放，缩放地图可能就导致某一个区间与其他区间不连通，
// 而这段代码里面没有对 next_node 进行初始化，假设由于缩放导致初始区间跟其他所有区间都不连通，
// 则在执行完 for 循环后，next_node 的值是一个随机值，从而可能导致运行到 visited[next_node] = true; 这里发生程序崩溃。
std::vector<int> NearestNeighborTSPSolver::solveNearestTSP(const cv::Mat &path_length_matrix, const int start_node)
{
    std::vector<int> calculated_order; //solution order
    // check if clique has more than one member or else this algorithm produces a order of size=3
    // 检查clique是否有多于一个成员，否则该算法将生成大小为3的顺序
    if (path_length_matrix.rows > 1)
    {
        int last_node; // index of the last spectated node
        std::vector<bool> visited(path_length_matrix.rows, false);
        // index of the current spectated node
        int current_node = start_node;
        calculated_order.push_back(current_node);
        visited[current_node] = true;
        // check every Point for the next nearest neighbor and add it to the order
        do
        {
            // saver for next node
            int next_node;
            // saver for distance to current next node
            double min_distance = 1e100;
            for (int current_neighbor = 0; current_neighbor < path_length_matrix.cols; current_neighbor++)
            {
                // check if current neighbor hasn't been visited yet
                if (visited[current_neighbor] == false)
                {
                    const double length = path_length_matrix.at<double>(current_node, current_neighbor);
                    if (length < min_distance && length > 0)
                    {
                        next_node = current_neighbor;
                        min_distance = length;
                    }
                }
            }
            // add the found nearest neighbor to the order-vector
            calculated_order.push_back(next_node);
            visited[next_node] = true;
            current_node = next_node;
        }
        // when the order has as many elements as the pathlength Matrix has the solver is ready
        while (calculated_order.size() < path_length_matrix.rows);
    }
    else calculated_order.push_back(start_node);
    return calculated_order;
}

// compute TSP and distance matrix without cleaning it
// this version does not exclude infinite paths from the TSP ordering
/** 这是一种通过计算最临近区域求出TSP近似解的方式。
 * 这是一种计算最邻近区域的方式。算法思想很简单，每次都取离当前位置最近的区域为下一个清扫区域，到达下一个区域后，再取最近的区域为下一个清扫区域。
 * @param original_map  是经过预处理以及角度旋转后的地图数据，用来计算点跟点之间的路径长度；
 * @param points  是前面做区间分割的时候，计算得出的每一个区间的中心点坐标数组；
 * @param downsampling_factor  则是地图的缩放比率，即把传入的地图按照给定的缩放比率进行缩放，在缩放后的地图上寻找路径，
 * 需要注意的是，这种方式虽然用来处理大地图的时候，可以提高计算速度，
 * 但是存在一个隐患： 当地图中存在窄通道时，缩放后该通道会闭合，从而导致两个区间不连通无法计算出区间距离；
 * @param robot_radius  是机器人的半径
 * @param map_resolution  栅格地图的分辨率
 * @param start_node  为初始区间序号，通过机器人当前坐标算出距离最近的区间
 * @param distance_matrix  则是用来返回距离矩阵。
 * @return
 */
std::vector<int> NearestNeighborTSPSolver::solveNearestTSP(
        const cv::Mat &original_map, const std::vector<cv::Point> &points,
        double downsampling_factor, double robot_radius, double map_resolution,
        const int start_node, cv::Mat *distance_matrix)
{
    std::cout << "NearestNeighborTSPSolver::solveNearestTSP: Constructing distance matrix..." << std::endl;
    // calculate the distance matrix
    // distance_matrix_ref 是一个二维的距离矩阵，该矩阵保存各个区间的两两距离，
    // 在进行距离计算时，只需计算二维矩阵对角右上方的两点距离，计算完毕即可得出所有的区间之间的最短距离。
    cv::Mat distance_matrix_ref;
    if (distance_matrix != 0) distance_matrix_ref = *distance_matrix;
    DistanceMatrix distance_matrix_computation;
    distance_matrix_computation.constructDistanceMatrix(distance_matrix_ref, original_map, points, downsampling_factor,
                                                        robot_radius, map_resolution, pathplanner_);
    return solveNearestTSP(distance_matrix_ref, start_node);
}

// compute TSP from a cleaned distance matrix (does not contain any infinity paths) that has to be computed
std::vector<int> NearestNeighborTSPSolver::solveNearestTSPClean(
        const cv::Mat &original_map, const std::vector<cv::Point> &points,
        double downsampling_factor, double robot_radius, double map_resolution,
        const int start_node)
{
    // compute a cleaned distance matrix
    cv::Mat distance_matrix_cleaned;
    std::map<int, int> cleaned_index_to_original_index_mapping;    // maps the indices of the cleaned distance_matrix to the original indices of the original distance_matrix
    int new_start_node = start_node;
    DistanceMatrix distance_matrix_computation;
    distance_matrix_computation.computeCleanedDistanceMatrix(original_map, points, downsampling_factor, robot_radius,
                                                             map_resolution, pathplanner_,
                                                             distance_matrix_cleaned,
                                                             cleaned_index_to_original_index_mapping, new_start_node);
    // solve TSP and re-index points to original indices
    return solveNearestTSPWithCleanedDistanceMatrix(distance_matrix_cleaned, cleaned_index_to_original_index_mapping,
                                                    new_start_node);
}

// compute TSP with pre-computed cleaned distance matrix (does not contain any infinity paths)
std::vector<int> NearestNeighborTSPSolver::solveNearestTSPWithCleanedDistanceMatrix(
        const cv::Mat &distance_matrix, const std::map<int, int> &cleaned_index_to_original_index_mapping, const int start_node)
{
    // solve TSP and re-index points to original indices
    std::vector<int> optimal_order = solveNearestTSP(distance_matrix, start_node);
    for (int & i : optimal_order) i = cleaned_index_to_original_index_mapping.at(i);
    return optimal_order;
}
